//
// Created by rebeater on 3/25/21.
//

#include "psins_run.h"
#include "PSINS.h"
#include "KFApp.h"
#include "WGS84.h"
#include "config.h"

CCarAHRS kf = CCarAHRS();

const double dt = 1. / default_opt.imu_data_rate;
float odo_var;
float odo_kd;
double init_gyro_bias[3] = {0., 0., 0.};
int bias_cnt = 0;
//CVect3 pos_pre = O31;
GnssData gnss_pre = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
static int gnss_week = 0;
/**
 * 判断GNSS数据是否有效
 * @param gnss
 * @return 0：无效
 *         1: 有效，低精度
 *         2：有效，高精度
 */
int isValid(GnssData *gnss) {
/*  if(gnss->mode >= 16){
    return 1;
  }else{
    return 0;
  }*/
  if (gnss->ns > 60) {/*同时收到60个卫星我认为是个bug*/
	return 0;
  }
  if (gnss->ns < 8) {/*低于5的抛弃*/
	return 0;
  }
  if (gnss->mode == SPP) {
	return 1;
  }
  if (gnss->mode == RTK_DGPS) {
	return 2;
  } else if (gnss->mode == RTK_FLOAT || gnss->mode == RTK_FIX) {
	return 3;
  } else {
	return 0;
  }
}

int kalmanInitialize() {
  Option opt = default_opt;
  kf.sins.eb = CVect3(init_gyro_bias);
//    kf.sins.eb = CVect3(opt.imuPara.gb_ini[0], opt.imuPara.gb_ini[1], opt.imuPara.gb_ini[2]);
  kf.sins.db = CVect3(opt.imuPara.ab_ini[0], opt.imuPara.ab_ini[1], opt.imuPara.ab_ini[2]);   // 加计-零偏
  kf.sins.sg = CVect3(opt.imuPara.gs_ini[0], opt.imuPara.gs_ini[1], opt.imuPara.gs_ini[2]);   //
  kf.sins.sa = CVect3(opt.imuPara.as_ini[0], opt.imuPara.as_ini[1], opt.imuPara.as_ini[2]);   // 加计-比例因子
  kf.sins.cg = I33;   // 陀螺-交叉耦合
  kf.sins.ca = I33;   // 加计-交叉耦合
  kf.sins.lb_gnss = CVect3(opt.antenna_arm[0], opt.antenna_arm[1], opt.antenna_arm[2]);//
  CVect3 pos_std = CVect3(1.0 / glv.Re,
						  1.0 / glv.Re,
						  1);     // variance of the initial position [m^2]   初始位置的标准差   RTK的话方差最大为0.1m
  CVect3 vel_std = CVect3(1, 1, 1);     // variance of the initial velocity [(m/s)^2]    初始速度的标准差
  CVect3 att_std = CVect3(2 * _deg, 2 * _deg, 5 * _deg);     // variance of the initial attitude [rad^2]
  double gb_std = opt.imuPara.gb_std;
  auto ab_std = opt.imuPara.ab_std;

  // P、Q矩阵的设置
  /*固定比例因子*/
  CVect3 imu_att = CVect3(0 * _deg, 0, 0);
  kf.sins.C_bv = CMat3(imu_att);
  kf.sins.lb_odo = CVect3(0, 0, 0);
  kf.sins.sf_odom = opt.odo_kd;
  odo_var = opt.odo_var;
  odo_kd = opt.odo_kd;
  kf.SetPk(att_std, vel_std, pos_std, gb_std, ab_std, 0.00);
  auto arw = opt.imuPara.arw;
  auto vrw = opt.imuPara.vrw;
  kf.SetQt(arw, vrw, 0.0, gb_std, opt.imuPara.gt_corr, ab_std, opt.imuPara.at_corr, 0.0000001, 3600);
  return 0;
}




//GnssMode gnss_mode;

/**
 * 根据位置东对准，返回值是速度
 * @param gnss
 * @param imu
 * @return
 */
double kalmanAlignPos(GnssData *gnss, ImuData *imu) {
  if (isValid(gnss) < 1) {
	return 0;
  }
  double vel = 0.;
  if (gnss_pre.mode < 1) {
	gnss_pre = *gnss;
	return 0;
  }
  gnss_week = gnss->week;
  CVect3 pos_cur, pos_pre, vm, wm;
  pos_cur = CVect3{gnss->lat * _deg, gnss->lon * _deg, gnss->height};
  pos_pre = CVect3{gnss_pre.lat * _deg, gnss_pre.lon * _deg, gnss_pre.height};
  vm = CVect3(imu->acce);
  wm = CVect3(imu->gyro);
  vm *= (wgs84.g * dt);/*加速度单位是1，需要转换一下*/
  wm *= (dt);/*都轉換爲增量形式*/
  /*不知道这个函数参数给第三个是要干嘛*/
  kf.measGPSVn = pp2vn(pos_cur, pos_pre, 1, nullptr);
  vel = normXY(kf.measGPSVn);
  if (vel > 100) {
	/*非正常速度*/
	return 0;
  }
  kf.Align_InMotion(wm, vm, 30, dt);
  kf.sins.vn = kf.measGPSVn;
  kf.sins.pos = pos_cur;
  kf.measGPSVn = O31;/*TODO Warning 这一步必须要 fuck!*/
  gnss_pre = *gnss;

  return vel;
}

extern Bias bias;
int imu_cnt;

int kalmanAlignLevel(ImuData *imu) {
  CVect3 vm, wm;
  vm = CVect3(imu->acce);
  wm = CVect3(imu->gyro);
  vm *= (wgs84.g * dt);/*加速度单位是1，需要转换一下*/
  wm *= (dt);/*都轉換爲增量形式*/
  wm = kf.sins.compensate(wm, kf.sins.eb * dt, kf.sins.sg, kf.sins.cg);   //补偿
  vm = kf.sins.compensate(vm, kf.sins.db * dt, kf.sins.sa, kf.sins.ca);
  for (int i = 0; i < 3; i++) init_gyro_bias[i] += imu->gyro[i];
  bias_cnt++;
  kf.sins.qnb = kf.align.Update(FALSE, &wm, &vm, 1, dt);
  if (bias_cnt > 10 / dt) {/* 60 * 204.8Hz = 60s*/
	kf.levelAlignOK = TRUE;
	for (double &init_gyro_bia : init_gyro_bias) init_gyro_bia /= bias_cnt;
  }
  kf.sins.att = CVect3{kf.sins.qnb};;
  return (kf.levelAlignOK == TRUE) && (kf.yawAlignOK == TRUE);
}
/**
 * 双天线对准航向
 * @param gnss
 * @return
 */
int kalmanAlignYawByDoubleAntenna(const GnssData *gnss, const ImuData *imu) {
  if (gnss->yaw > 360 || gnss->yaw < 0) {
	return -1;
  }
/*  CVect3 wm, vm;
  vm = CVect3(imu->acce) * wgs84.g * dt;
  wm = CVect3(imu->gyro)*dt;
  wm = kf.sins.compensate(wm, kf.sins.eb * dt, kf.sins.sg, kf.sins.cg);   //补偿
  vm = kf.sins.compensate(vm, kf.sins.db * dt, kf.sins.sa, kf.sins.ca);
  for (int i = 0; i < 3; i++) init_gyro_bias[i] += imu->gyro[i];
  kf.align.Update(TRUE,&wm,&vm,1,dt);
  bias_cnt ++;
  if (bias_cnt > 10 * 128) {*//* 60 * 204.8Hz = 60s*//*
	kf.levelAlignOK = TRUE;
	for (double &init_gyro_bia : init_gyro_bias) init_gyro_bia /= bias_cnt;
  }*/
  double gb_std = default_opt.imuPara.gb_std;
  double ab_std = default_opt.imuPara.ab_std;
  auto att = CVect3(kf.sins.qnb);
  att.k = gnss->yaw * _deg;
  kf.sins.att = att;
  kf.sins.qnb = CQuat(kf.sins.att);
  kf.sins.pos = CVect3(gnss->lat * _deg, gnss->lon * _deg, gnss->height);
  kf.SetPk({1 * _deg, 1 * _deg, gnss->yaw_std_100 * _deg / 100},
		   {2, 2, 2},
		   {gnss->pos_std[0] / glv.Re, gnss->pos_std[1] / glv.Re, gnss->pos_std[2]},
		   gb_std, ab_std, 0.00);
  kf.sins.vn = O31;
  kf.yawAlignOK = TRUE;
  return kf.levelAlignOK && kf.yawAlignOK;
}

/*零速检测*/
void kalmanZeroVelocityDetect() {

}
int kalmanAlignUseDefualt() {
  NavPva &epoch = initial_epoch;
  auto att0 = CVect3(epoch.pitch, epoch.roll, epoch.yaw);
  att0.k = att0.k > PI ? 2 * PI - att0.k : -att0.k;   // Yaw 真值北偏东为正(0, 360)→北偏西为正(-180,180）
  kf.sins.att = att0;
  kf.sins.qnb = CQuat(kf.sins.att);
  att0 = att0 / glv.deg;
//    std::cout << "Initial align att = " << att0.i << ',' << att0.j << ',' << att0.k << std::endl;
  kf.sins.pos = CVect3(epoch.lat, epoch.lon, epoch.h);
  kf.sins.vn = CVect3(epoch.ve, epoch.vn, -epoch.vd);
//    printf("init height = %f\n", kf.sins.pos.k);
//    init_height = kf.sins.pos.k;
  wgs84.update_g(kf.sins.pos.i, kf.sins.pos.k);
  kf.yawAlignOK = TRUE;
  return 0;
}

CVect3 wm1{0, 0, 0}, vm1{0, 0, 0};

PSINS_STATE kalmanUpdate(ImuData *imu) {
  CVect3 vm{imu->acce}, wm{imu->gyro};
  vm *= (wgs84.g * dt);/*加速度单位是1，需要转换一下*/
  wm *= (dt);/*都轉換爲增量形式*/
  wm = kf.sins.compensate(wm, kf.sins.eb * dt, kf.sins.sg, kf.sins.cg);   //补偿
  vm = kf.sins.compensate(vm, kf.sins.db * dt, kf.sins.sa, kf.sins.ca);
  /*ZUPT and NHC should be here*/
  kf.Update(wm1, vm1, wm, vm, dt);
  vm1 = vm;
  wm1 = wm;
  return PSINS_OK;
}

PSINS_STATE kalmanSetGNSS(GnssData *gnss, double dt) {
  gnss_week = gnss->week;
  auto gpsPos = CVect3{gnss->lat * _deg, gnss->lon * _deg, gnss->height};
  auto gpsStd = CVect3{gnss->pos_std[0] / glv.Re, gnss->pos_std[1] / glv.Re, gnss->pos_std[2]};

  kf.SetMeasGPSPos(gpsPos, dt);
  kf.SetRk_GPSPOS(gpsStd);
//    gnss_mode = static_cast<GnssMode>((gnss->mode) & 0x00ffu);
  if (gnss->mode == RTK_FIX)/*固定解时更新g值*/
	wgs84.update_g(gnss->lat * _deg, gnss->height);
  return PSINS_OK;
}

int kalmanOutput(NavOutput *nav_output) {
  nav_output->lat = kf.sins.pos.i / _deg;
  nav_output->lon = kf.sins.pos.j / _deg;
  nav_output->height = (float)kf.sins.pos.k;
  /*转换成float降低文件大小*/
  nav_output->vn[0] = (float)kf.sins.vn.i;
  nav_output->vn[1] = (float)kf.sins.vn.j;
  nav_output->vn[2] = (float)kf.sins.vn.k;
  nav_output->atti[0] = (float)(kf.sins.att.j / _deg);
  nav_output->atti[1] = (float)(kf.sins.att.i / _deg);
  nav_output->atti[2] = -(float)(kf.sins.att.k / _deg);/*转换到了北东地坐标系*/
  //  nav_output->pos_std[0] = kf.si
  return 1;
}
void GetBias(short gb[3],short ab[3]){
  gb[0] = (short) (kf.sins.eb.i/_deg*_hour);
  gb[1] = (short) (kf.sins.eb.j/_deg*_hour);
  gb[2] = (short) (kf.sins.eb.k/_deg*_hour);
  ab[0] = (short) (kf.sins.db.i/_mGal);
  ab[1] = (short) (kf.sins.db.j/_mGal);
  ab[2] = (short) (kf.sins.db.k/_mGal);
}

PSINS_STATE alignUseGiven() {
  NavPva &epoch = initial_epoch;
  auto att0 = CVect3(epoch.pitch, epoch.roll, epoch.yaw);
  att0.k = att0.k > PI ? 2 * PI - att0.k : -att0.k;   // Yaw 真值北偏东为正(0, 360)→北偏西为正(-180,180）
  kf.sins.att = att0;
  kf.sins.qnb = CQuat(kf.sins.att);
  kf.sins.pos = CVect3(epoch.lat, epoch.lon, epoch.h);
  kf.sins.vn = CVect3(epoch.ve, epoch.vn, -epoch.vd);
  kf.yawAlignOK = TRUE;
  kf.levelAlignOK = TRUE;
  return PSINS_OK;
}

PSINS_STATE kalmanSetVel(float vel) {
  CVect3 rk{0.01, 0.01, 0.001};
  if (vel < 0.00001) {
	kf.SetMeasZUPT();
	kf.SetRk_ZUPT(rk, 0.1);
	return PSINS_ZUPT;
  }
  CVect3 v{0, odo_kd * vel, 0};
  CVect3 std2{odo_var, odo_var, odo_var};
  kf.SetMeasODOVN(v);
  kf.SetRk_ODO(std2);
  return PSINS_OK;
}
